#include "robot_arm_kinematics.h"
#include <math.h>
#include <string.h>
#include "Usart.h"
#include <stdio.h>
#include "NeZha.h"
#include <stdlib.h>

// 图像标定定位坐标
#define CENTER_X 160 // 根据实际图像分辨率调整
#define CENTER_Y 200

// 偏差允许范围
#define X_OFFSET_THRESHOLD 20
#define Y_OFFSET_THRESHOLD 30
#define S_THRESHOLD        1000

// 电机最大速度
#define MAX_SPEED 150

bool ShouldTriggerGripper(void)
{
    static GripperState state      = GRIPPER_IDLE;
    static uint8_t stable_counter  = 0;
    const uint8_t STABLE_THRESHOLD = 3;  // 连续3帧稳定才触发
    const uint16_t CENTER_X_MIN    = 45; // QQVGA2中心区域定义
    const uint16_t CENTER_X_MAX    = 65;
    const uint16_t CENTER_Y_MIN    = 45;
    const uint16_t CENTER_Y_MAX    = 75;

    SpitballCoordinates current = Get_Spitball_Coordinates();

    // 数据有效性校验（排除0值及超范围值）
    if (current.x == 0 || current.y == 0 ||
        current.x > 160 || current.y > 120) {
        return false;
    }

    // 状态机处理
    switch (state) {
        case GRIPPER_IDLE:
            if ((current.x >= CENTER_X_MIN) && (current.x <= CENTER_X_MAX) &&
                (current.y >= CENTER_Y_MIN) && (current.y <= CENTER_Y_MAX)) {
                stable_counter++;
                if (stable_counter >= STABLE_THRESHOLD) {
                    stable_counter = 0;
                    state          = GRIPPER_PENDING;
                    return true; // 满足条件触发抓取
                }
            } else {
                stable_counter = 0;
            }
            break;

        case GRIPPER_PENDING:
            // 等待机械臂完成抓取的过渡状态
            if ((current.x < (CENTER_X_MIN - 10)) || (current.x > (CENTER_X_MAX + 10)) ||
                (current.y < (CENTER_Y_MIN - 10)) || (current.y > (CENTER_Y_MAX + 10))) {
                state = GRIPPER_IDLE; // 物体移出区域后重置
            }
            break;
    }

    return false;
}

// 判断是否到达中心并移动小车的函数
M_Target CheckAndMoveToCenter(SpitballCoordinates coords)
{
    M_Target MTarget;
    int base_speed = 150; // 基础速度
    // 计算偏差
    int x_offset = coords.x - CENTER_X;
    int y_offset = coords.y - CENTER_Y;
    if (coords.x == -1 && coords.y == -1) {
        // 未检测到目标，寻找目标
        MTarget.m1 = 0;
        MTarget.m2 = 0;
        MTarget.m3 = 0;
        MTarget.m4 = 0;
        return MTarget;
    } else if (coords.x != -1 && coords.y != -1 && coords.s < S_THRESHOLD) { // 判断是否在允许偏差范围内
        if (coords.y < 170) {
            MTarget.m1 = -base_speed;
            MTarget.m2 = base_speed;
            MTarget.m3 = base_speed;
            MTarget.m4 = -base_speed;
            return MTarget;
        } else if (abs(x_offset) > X_OFFSET_THRESHOLD && abs(y_offset) > Y_OFFSET_THRESHOLD) {
            MTarget.m1 = 0;
            MTarget.m2 = 0;
            MTarget.m3 = 0;
            MTarget.m4 = 0;
            return MTarget;
        }
    } else if (abs(x_offset) < X_OFFSET_THRESHOLD) { // 到达中心，停止电机
        MTarget.m1 = 0;
        MTarget.m2 = 0;
        MTarget.m3 = 0;
        MTarget.m4 = 0;
        return MTarget;
    } else if (x_offset < 0) {
        MTarget.m1 = base_speed;
        MTarget.m2 = base_speed;
        MTarget.m3 = base_speed;
        MTarget.m4 = base_speed;
        return MTarget;
    } else if (x_offset > 0) {
        MTarget.m1 = -base_speed;
        MTarget.m2 = -base_speed;
        MTarget.m3 = -base_speed;
        MTarget.m4 = -base_speed;
        return MTarget;
    } else {
        MTarget.m1 = 0;
        MTarget.m2 = 0;
        MTarget.m3 = 0;
        MTarget.m4 = 0;
        return MTarget;
    }
}
